#include "vtor_modbus.h"

#ifdef __VTOR_MODBUS__

// 默认id，默认寄存器列表
uint8_t VtorModbus_id = 0x01;
VtorModbusRegister* VtorModbus_registerArray = NULL;

static uint16_t* VtorModbus_registerBuf = NULL;
uint16_t VtorModbusMaster_readRegAddr = 0;
uint8_t VtorModbus_txBuf[128] = {0};
uint8_t VtorModbus_txLen = 0;


uint8_t VtorModbusMaster_ReadReg(uint8_t slaveId, uint16_t regAddr, uint16_t* regBuf, uint16_t regNum)
{
	// 调整从机的id与数据存放地址，稍后校验
	VtorModbus_id = slaveId;
	VtorModbus_registerBuf = regBuf;
	VtorModbusMaster_readRegAddr = regAddr;
	// 开始封装
	VtorModbus_txBuf[0] = slaveId;
	
	
	if (regAddr >= VtorModbus_ReadWriteStartAddr
		&& regAddr <= VtorModbus_ReadWriteEndAddr)
	{
		regAddr -= VtorModbus_ReadWriteStartAddr;
		VtorModbus_txBuf[1] = VtorModbus_ReadReadWriteRegister;
	}
	else if (regAddr >= VtorModbus_ReadOnlyStartAddr
		&& regAddr <= VtorModbus_ReadOnlyEndAddr)
	{
		regAddr -= VtorModbus_ReadOnlyStartAddr;
		VtorModbus_txBuf[1] = VtorModbus_ReadReadOnlyRegister;
	}
	else
	{
		return 1;
	}
	VtorModbus_txBuf[2] = regAddr >> 8;
	VtorModbus_txBuf[3] = regAddr & 0xff;
	VtorModbus_txBuf[4] = regNum >> 8;
	VtorModbus_txBuf[5] = regNum & 0xff;
	VtorModbus_AppendCrc(VtorModbus_txBuf, 6);
    VtorModbus_txLen = 6 + 2;
	VtorModbus_SendBuffer(VtorModbus_txBuf, 8);
	return 0;
}

uint8_t VtorModbusMaster_WriteReg(uint8_t slaveId, uint16_t regAddr, uint16_t* regBuf, uint16_t regNum)
{
	int16_t regIdx = 0;
	// 调整从机的id与数据存放地址，稍后校验
	VtorModbus_id = slaveId;
	VtorModbus_registerBuf = NULL;
	
	// 开始封装
	VtorModbus_txBuf[0] = slaveId;
	
	
	if (regAddr >= VtorModbus_ReadWriteStartAddr
		&& regAddr <= VtorModbus_ReadWriteEndAddr)
	{
		regAddr -= VtorModbus_ReadWriteStartAddr;
		VtorModbus_txBuf[1] = VtorModbus_WriteMultipleRegister;
	}
	else
	{
		return 1;
	}

	VtorModbus_txBuf[2] = regAddr >> 8;
	VtorModbus_txBuf[3] = regAddr & 0xff;
	VtorModbus_txBuf[4] = regNum >> 8;
	VtorModbus_txBuf[5] = regNum & 0xff;
	
	VtorModbus_txBuf[6] = regNum << 1;
	
	for(regIdx = 0; regIdx < regNum; regIdx++)
	{
		VtorModbus_txBuf[7 + 0 + 2 * regIdx] = regBuf[regIdx] >> 8;
		VtorModbus_txBuf[7 + 1 + 2 * regIdx] = regBuf[regIdx];
	}
	VtorModbus_AppendCrc(VtorModbus_txBuf, 7 + regNum * 2);
    VtorModbus_txLen = 7 + regNum * 2 + 2;
	VtorModbus_SendBuffer(VtorModbus_txBuf, 7 + regNum * 2 + 2);
	return 0;
}

uint8_t VtorModbusSlave_ReadReg(uint16_t regAddr, uint16_t* regBuf, uint16_t regNum)
{
	int i = 0;
	VtorModbusRegister* reg = VtorModbus_SearchAddress(regAddr);
	if (NULL == reg)
	{
		return 1;
	}
	for (i = 0; i < regNum && reg->addr != 0; i++)
	{
		regBuf[i] = reg->value;
		reg++;
	}
	return 0;
}

uint8_t VtorModbusSlave_WriteReg(uint16_t regAddr, uint16_t* regBuf, uint16_t regNum)
{
	int i = 0;
	VtorModbusRegister* reg = VtorModbus_SearchAddress(regAddr);
	if (NULL == reg)
	{
		return 1;
	}
	for (i = 0; i < regNum && reg->addr != 0; i++)
	{
		reg->value = regBuf[i];
		reg++;
	}
	return 0;
}

uint8_t VtorModbusSlave_ReadFloat(uint16_t regAddr, float* floatBuf, uint16_t floatNum)
{
	int i = 0;
	VtorModbusRegister* reg = VtorModbus_SearchAddress(regAddr);
	if (NULL == reg)
	{
		return 1;
	}
	for (i = 0; i < floatNum && reg->addr != 0; i++)
	{
		*((uint16_t*)&floatBuf[i] + 1) = reg->value;
		reg++;
		*((uint16_t*)&floatBuf[i] + 0) = reg->value;
		reg++;
	}
	return 0;
}

uint8_t VtorModbusSlave_WriteFloat(uint16_t regAddr, float* floatBuf, uint16_t floatNum)
{
	int i = 0;
	VtorModbusRegister* reg = VtorModbus_SearchAddress(regAddr);
	if (NULL == reg)
	{
		return 1;
	}
	for (i = 0; i < floatNum && reg->addr != 0; i++)
	{
		reg->value = *((uint16_t*)&floatBuf[i] + 1);
		reg++;
		reg->value = *((uint16_t*)&floatBuf[i] + 0);
		reg++;
	}
	return 0;
}

VtorModbusRegister* VtorModbus_SearchAddress(uint16_t addr)
{
	VtorModbusRegister* reg = VtorModbus_registerArray;
	while(0 != reg->addr)
	{
		if(reg->addr == addr)
		{
			return reg;
		}
		reg++;
	}
	return NULL;
}

#ifdef __VTOR_MODBUS_CONFIG_CRC_CALC__
uint16_t VtorModbus_CalcCrc(uint8_t* buf, uint16_t dataLen)
{
	uint16_t TCPCRC = 0xffff;
	uint16_t POLYNOMIAL = 0xa001;
	uint8_t i, j;
	for (i = 0; i < dataLen; i++)
	{
		TCPCRC ^= buf[i];
		for (j = 0; j < 8; j++)
		{
			if ((TCPCRC & 0x0001) != 0)
			{
				TCPCRC >>= 1;
				TCPCRC ^= POLYNOMIAL;
			}
			else
			{
				TCPCRC >>= 1;
			}
		}
	}
	return TCPCRC;
}

#endif

#ifdef __VTOR_MODBUS_CONFIG_CRC_NULL__

uint16_t VtorModbus_CalcCrc(uint8_t* buf, uint16_t dataLen)
{
	return 0;
}

#endif

#ifdef __VTOR_MODBUS_CONFIG_CRC_TABLE__

// https://blog.csdn.net/qq_35358125/article/details/106296169
//查表方式实现
static const uint8_t aucCRCHi[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40
};

static const uint8_t aucCRCLo[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,
0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,
0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
0x41, 0x81, 0x80, 0x40
};

uint16_t VtorModbus_CalcCrc(uint8_t* buf, uint16_t dataLen)
{
	uint8_t ucCRCHi = 0xFF;
	uint8_t ucCRCLo = 0xFF;
	int iIndex;
	while( dataLen-- )
	{
		iIndex = ucCRCLo ^ *( buf++ );
		ucCRCLo = ( uint8_t )( ucCRCHi ^ aucCRCHi[iIndex] );
		ucCRCHi = aucCRCLo[iIndex];
	}
	return (ucCRCHi << 8) | ucCRCLo;
}

#endif

void VtorModbus_AppendCrc(uint8_t* buf, uint16_t dataLen)
{
	uint16_t calcCrc = VtorModbus_CalcCrc(buf, dataLen);
	buf[dataLen + 0] = calcCrc & 0xff;
	buf[dataLen + 1] = calcCrc >> 8;
}

uint8_t VtorModbus_CheckCrc(uint8_t* buf, uint16_t totalLen)
{
	//https://blog.csdn.net/qq_35358125/article/details/106296169
	uint16_t calcCrc = VtorModbus_CalcCrc(buf, totalLen - 2);
	
	// 正确返回1，失败返回0
	// 低字节存储低位，高字节存储高位
	if ((buf[totalLen - 2] == (calcCrc & 0xff))
		&& (buf[totalLen - 1] == calcCrc >> 8))
	{
		return 1;
	}
	return 0;
}

uint16_t VtorModbusMaster_Process(uint8_t* rxBuf, uint16_t len)
{
	uint8_t i = 0;
	uint8_t id = rxBuf[0];
	uint8_t cmd = rxBuf[1];
	uint16_t regNum = rxBuf[2] >> 1;
	if(len < 6)
	{
		return 3;
	}
	if(VtorModbus_id != id && id != 0)
	{
		return 2;
	}
	if (0 == VtorModbus_CheckCrc(rxBuf, len))
	{
		return 1;
	}
	if (cmd == VtorModbus_ReadReadWriteRegister
		|| cmd == VtorModbus_ReadReadOnlyRegister)
	{
		if(VtorModbus_registerBuf != NULL)
		{
			for(i = 0; i < regNum; i++)
			{
				VtorModbus_registerBuf[i]
					= (rxBuf[3 + 0 + i * 2] << 8) | rxBuf[3 + 1 + i * 2];
			}
			VtorModbusMaster_ProcessReadCallback(VtorModbusMaster_readRegAddr, regNum);
		}
		// 数据获取完成，将其指向空
		VtorModbus_registerBuf = NULL;
	}
	return 0;
}

uint16_t VtorModbusSlave_Process(uint8_t* rxBuf, uint16_t len)
{
	uint16_t replyLen = 0, j = 0;
	uint8_t id = rxBuf[0];
	uint8_t cmd = rxBuf[1];
	uint16_t regAddr = rxBuf[2] << 8 | rxBuf[3];
	uint16_t regNum = rxBuf[4] << 8 | rxBuf[5];
	VtorModbusRegister* reg = NULL;

	if(len < 6)
	{
		return 3;
	}
	if (0 == VtorModbus_CheckCrc(rxBuf, len))
	{
		return 1;
	}
	if(VtorModbus_id != id && id != 0)
	{
		return 2;
	}
	if (cmd == VtorModbus_ReadReadWriteRegister
		|| cmd == VtorModbus_WriteSingleRegister
		|| cmd == VtorModbus_WriteMultipleRegister)
	{
		regAddr += VtorModbus_ReadWriteStartAddr;
	}
	else if (cmd == VtorModbus_ReadReadOnlyRegister)
	{
		regAddr += VtorModbus_ReadOnlyStartAddr;
	}
	else
	{
		return 3;
	}

	reg = VtorModbus_SearchAddress(regAddr);
	if(NULL == reg)
	{
		return 4;
	}


	VtorModbus_txBuf[replyLen++] = VtorModbus_id; // 返回时使用自己的id
	VtorModbus_txBuf[replyLen++] = rxBuf[1]; // 功能码
	if (cmd == VtorModbus_ReadReadWriteRegister
		|| cmd == VtorModbus_ReadReadOnlyRegister)
	{
		VtorModbus_txBuf[replyLen++] = 2 * regNum;
		for(j = 0; j < regNum && reg->addr != 0; j++)
		{
			VtorModbus_txBuf[replyLen++] = reg->value >> 8;
			VtorModbus_txBuf[replyLen++] = reg->value & 0xff;
			reg++;
		}
	}
	else if (cmd == VtorModbus_WriteSingleRegister)
	{
		regNum = 1;
		VtorModbus_txBuf[replyLen++] = rxBuf[2];
		VtorModbus_txBuf[replyLen++] = rxBuf[3];

		reg->value = rxBuf[4] << 8 | rxBuf[5];
		VtorModbus_txBuf[replyLen++] = reg->value >> 8;
		VtorModbus_txBuf[replyLen++] = reg->value & 0xff;
		VtorModbusSlave_ProcessWriteCallback(regAddr, regNum);
	}
	else if (cmd == VtorModbus_WriteMultipleRegister)
	{
	  // 起始地址与长度信息
		VtorModbus_txBuf[replyLen++] = rxBuf[2]; // 寄存器首地址
		VtorModbus_txBuf[replyLen++] = rxBuf[3];
		VtorModbus_txBuf[replyLen++] = rxBuf[4]; // 寄存器个数
		VtorModbus_txBuf[replyLen++] = rxBuf[5];
		
		regNum = rxBuf[4] << 8 | rxBuf[5];

		for(j = 0; j < regNum && reg->addr != 0; j++)
		{
			reg->value = rxBuf[7 + j * 2] << 8 | rxBuf[7 + j * 2 + 1];
			reg++;
		}
		VtorModbusSlave_ProcessWriteCallback(regAddr, regNum);
	}
	VtorModbus_AppendCrc(VtorModbus_txBuf, replyLen);
	VtorModbus_SendBuffer(VtorModbus_txBuf, replyLen + 2);
	return 0;
}


#endif // __VTOR_MODBUS__
